


// static const char WINDOW[] = "Image window";

class ImageConverter {
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;

public:
  ImageConverter() :
      it_(nh_) {
    image_pub_ = it_.advertise("out", 1);
    image_sub_ = it_.subscribe("/camera/rgb/image_color", 1, &ImageConverter::imageCb, this);
    // image_sub_ = it_.subscribe("/image_raw", 1, &ImageConverter::imageCb, this);

    cv::namedWindow(WINDOW);
  }

  ~ImageConverter() {
    cv::destroyWindow(WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg) {
    cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

// IplImage* img = cvCloneImage(&input);

//     IplImage* imgHSV = cvCloneImage(img);

//     // IplImage* imgHSV = cvCreateImage(cvGetSize(img),8,3);

//     cvCvtColor(img, imgHSV, CV_BGR2HSV);


    IplImage input(cv_ptr->image);

    IplImage* img = cvCloneImage(&input);

    IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);

    cvCvtColor(img, imgHSV, CV_BGR2HSV);

    IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);

    // Orange ball for kinect
    cvInRangeS(imgHSV, cvScalar(10, 155, 108), cvScalar(22, 255, 255), imgThreshed);

    // Orange ball for webcam    
    // cvInRangeS(imgHSV, cvScalar(5, 80, 90), cvScalar(22, 255, 255), imgThreshed);

    RNG rng(12345);

    /// Read the image
    Mat src = Mat(imgThreshed, true);
    Mat drawing = Mat::zeros(src.size(), CV_8UC3);

    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;

    // Find contours
    cv::findContours(src, contours, hierarchy, CV_RETR_TREE,
        CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));

    /// Get the moments

    double area = 0.0;
    double max_area = 0.0;
    int max_index = -1;

    for (int i = 0; i < contours.size(); ++i) {

      area = contourArea(contours[i]);

      if (area <= 30) {
        continue;
      }

      if (area > max_area) {
        max_area = area;
        max_index = i;
      }

    }

    if (max_index < 0) {

      ROS_ERROR("max_index = %d", max_index);

    } else {


      Moments mu = moments(contours[max_index], false);
      Point2f p2f = Point2f(mu.m10 / mu.m00, mu.m01 / mu.m00);

      // drawContours( drawing, contours, max_index, cv::Scalar(255,0,0), 2, 8, hierarchy, 0, Point() );

      circle(src, p2f, 30, Scalar(255, 0, 0), 3, 8, 0);

      ROS_INFO("p2f = [x,y] = [%d,%d]", int(p2f.x), int(p2f.y));

    }

    // cvShowImage(WINDOW,imgThreshed);


    cv::imshow(WINDOW, src);

    cvReleaseImage(&img);
    cvReleaseImage(&imgHSV);
    cvReleaseImage(&imgThreshed);

    // if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
    //   cv::circle(cv_ptr->image, cv::Point(300, 300), 100, CV_RGB(255,0,0));

    // cv::imshow(WINDOW, cv_ptr->image);
    cv::waitKey(3);

    image_pub_.publish(cv_ptr->toImageMsg());

  }
};

int main(int argc, char** argv) {
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}

// Colors:

// cvInRangeS(imgHSV,cvScalar(10,100,210), cvScalar(30,105,255),imgThreshed);

// orange-red
// cvInRangeS(imgHSV,cvScalar(10,135,155), cvScalar(20,255,255),imgThreshed);

// bright green:
// cvInRangeS(imgHSV,cvScalar(53,74,160), cvScalar(90,147,255),imgThreshed);

// Mat
// circle( src, Point(50,50), 100, Scalar(255,0,0), 3, 8, 0 );
